import sys
from PyQt5.QtCore import Qt, QTimer
from PyQt5.QtWidgets import *

import rclpy
from .ros_backend import ROS2Node


class MainWindow(QWidget):
    def __init__(self):
        super().__init__()
        self.initUI()
        self.ros: ROS2Node = ROS2Node(self.close)
        self.rosTimer = QTimer()
        self.rosTimer.start(10)
        self.rosTimer.timeout.connect(self.ros.onTick)
        self.lastMode: str = "None"

    def initUI(self):
        self.setWindowTitle("CommandCentral")

        self.refreshButton = QPushButton("Refresh")
        self.refreshButton.clicked.connect(self.OnRefresh)
        self.instancePicker1 = QComboBox()
        self.instancePicker2 = QComboBox()
        instanceLayout = QHBoxLayout()
        instanceLayout.addWidget(self.instancePicker1)
        instanceLayout.addWidget(self.instancePicker2)
        instanceLayout.addWidget(self.refreshButton)

        self.progressBar = QProgressBar()

        self.modePicker = QComboBox()
        self.modePicker.addItem("RC Mode")
        self.modePicker.addItem("Balance")
        self.modePicker.addItem("Auto Dock")
        self.modePicker.addItem("Disconnect")
        self.modePicker.addItem("Close")
        self.startButton = QPushButton("Invoke")
        self.startButton.clicked.connect(self.OnStart)
        self.stopButton = QPushButton("Stop")
        self.stopButton.clicked.connect(self.OnStop)
        self.modeLayout = QHBoxLayout()
        self.modeLayout.addWidget(self.modePicker)
        self.modeLayout.addWidget(self.startButton)
        self.modeLayout.addWidget(self.stopButton)

        mainlayout = QVBoxLayout(self)
        mainlayout.addLayout(instanceLayout)
        mainlayout.addWidget(self.progressBar)
        mainlayout.addLayout(self.modeLayout)

        self.setLayout(mainlayout)

    def OnStart(self):
        buttonStateChanged = False
        self.lastMode = self.modePicker.currentText()
        if self.lastMode == "RC Mode":
            self.ros.RCMode(self.instancePicker1.currentText(), True)
            buttonStateChanged = True
        elif self.lastMode == "Auto Dock":
            if self.instancePicker1.currentText() == self.instancePicker2.currentText():
                QErrorMessage(self).showMessage(
                    "Instance can not be docked with itself"
                )
            else:
                self.ros.AutoDockMode(
                    self.instancePicker1.currentText(),
                    self.instancePicker2.currentText(),
                    True,
                )
                buttonStateChanged = True
        elif self.lastMode == "Balance":
            self.ros.BalanceMode(self.instancePicker1.currentText())
        elif self.lastMode == "Disconnect":
            self.ros.DisconnectMode(self.instancePicker1.currentText())
        elif self.lastMode == "Close":
            self.ros.CloseControl(self.instancePicker1.currentText())
        else:
            QErrorMessage(self).showMessage("Not Implemented")
        if buttonStateChanged:
            self.stopButton.setEnabled(True)
            self.startButton.setEnabled(False)
            self.instancePicker1.setEnabled(False)
            self.instancePicker2.setEnabled(False)

    def OnStop(self):
        self.stopButton.setEnabled(False)
        self.startButton.setEnabled(True)
        self.instancePicker1.setEnabled(True)
        self.instancePicker2.setEnabled(True)
        if self.lastMode == "RC Mode":
            self.ros.RCMode(self.instancePicker1.currentText(), False)
        elif self.lastMode == "Auto Dock":
            self.ros.AutoDockMode(
                self.instancePicker1.currentText(),
                self.instancePicker2.currentText(),
                False,
            )
        else:
            QErrorMessage(self).showMessage("Not Implemented")

    def OnRefresh(self):
        instances = self.ros.GetAllInstances()
        self.instancePicker1.clear()
        self.instancePicker2.clear()
        for instance in instances:
            self.instancePicker1.addItem(instance[0])
            self.instancePicker2.addItem(instance[0])

    def closeEvent(self, a0):
        self.ros.Info("close")
        self.ros.onDestory()
        return super().closeEvent(a0)


def main():
    rclpy.init()
    app = QApplication(sys.argv)
    mainWindow = MainWindow()
    mainWindow.show()
    sys.exit(app.exec_())


if __name__ == "__main__":
    main()
